// 正逆运动学测试
#include "../include/2Dof_Model_dyna.h"

#include <pinocchio/parsers/urdf.hpp> // 引入 URDF 解析器
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/multibody/model.hpp>

#include "ros/ros.h"

using std::cerr;
using std::cout;
using std::endl;
using std::exception;
using std::string;
using std::vector;
using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ikTest");
    ros::NodeHandle n;
    ros::Rate r(1000);

    UR2DOFARM DofModel;
    // 标准的DH参数
    float a_sd[2] = {0.1, 0.2};
    float alpha_sd[2] = {0, 0};
    float d_sd[2] = {0, 0};
    DofModel.SetDH_SD(a_sd, d_sd, alpha_sd);

    // 运动学模型
    pinocchio::Model model; // Pinocchio 机器人模型
    std::string model_path = "/home/koliko/ROS/arm_ws/src/urdf_7dof/urdf/urdf_7dof.urdf";
    pinocchio::urdf::buildModel(model_path, model);
    pinocchio::Data data(model);

    std::cout << "机械臂关节数: " << model.nq << std::endl;
    std::cout << "机械臂自由度: " << model.nv << std::endl;
    std::cout << "关节名称列表: " << std::endl;

    pinocchio::FrameIndex ee_id = model.getFrameId("link7_end"); // 获取 Frame ID
    Eigen::VectorXd q(model.nq);

    // 不同的关节角度

    q[0] = 0;
    q[1] = 0;
    q[2] = 0;
    q[3] = 1.57;
    q[4] = 0;
    q[5] = 0;
    q[6] = 0;

    std::cout << "关节角度 q: " << q.transpose() << std::endl;

    pinocchio::forwardKinematics(model, data, q);
    pinocchio::updateFramePlacements(model, data);

    Eigen::Vector3d ee_pos = data.oMf[ee_id].translation();
    std::cout << "Joint configuration q: " << q.transpose() << std::endl;
    std::cout << "末端位置: " << ee_pos.transpose() << std::endl;

    std::cout << "Model details: " << std::endl;
    std::cout << model << std::endl; // 打印模型的详细信息
    std::cout << "Joint 0 type: " << model.joints[0].shortname() << std::endl;

    // 雅可比矩阵测试
    q << 0, 0; // 示例关节角度
    // 进行正向运动学计算
    pinocchio::forwardKinematics(model, data, q);
    pinocchio::updateFramePlacements(model, data);

    // 获取末端执行器的 frame ID
    pinocchio::FrameIndex frame_id = model.getFrameId("link2_end");

    // 定义雅可比矩阵
    Eigen::MatrixXd J(6, model.nv);

    // 计算相对于世界坐标系的雅可比矩阵
    pinocchio::computeFrameJacobian(model, data, q, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, J);

    // 输出雅可比矩阵
    std::cout << "雅可比矩阵 J:\n"
              << J << std::endl;
}